/*
 * Center then drive to the end of the hall, 
 * turn around and return to the origional position.
 *
 * Current: check the sonar
 */

#include <libplayerc++/playerc++.h>
#include <iostream>

#include "args.h"

#define RAYS 32

int main(int argc, char **argv)
{
  parse_args(argc,argv);

  // we throw exceptions on creation if we fail
  try
  {
    using namespace PlayerCc;
    
    PlayerClient robot(gHostname, gPort);
    Position2dProxy pp(&robot, gIndex);
    LaserProxy lp(&robot, gIndex);
    SonarProxy sp(&robot, gIndex);
    std::cout << robot << std::endl;

    pp.SetMotorEnable (true);

    
    double newspeed = 0;
    double newturnrate = 0;
    //double minR = 1e9;
    //double minL = 1e9;
    //int lloc = 0;
    //int rloc = 0;
    //int temp;

    robot.Read(); //here so GetRange doesn't segfault
      
    // go into read-think-act loop
    for(;;)
    {

      // this blocks until new data comes; 10Hz by default
      robot.Read();

      /* Simple accessor functions
      std::cout << "Max Range of laser " << lp.GetMaxRange() << std::endl;
      std::cout << "Number of points in scan " << lp.GetCount() << std::endl;
      std::cout << "Bearing " << lp.GetBearing(135) << std::endl;
      getchar();
      */

      /* GetRange() 
      std::cout << "Testing GetRange \n"; double range;
      range = lp.GetRange(269); // far bottom left
      //range = lp.GetRange(0); // far bottom right
      //range = lp.GetRange(135); // directly in front
      std::cout << "Range: " << range << std::endl;
      range = lp[269];
      std::cout << "Using []: " << range << std::endl;
      getchar();
      */

      /* GetMinRight and GetMinLeft */
      std::cout << "Testing GetMinRight & GetMinLeft \n";
      double minR = sp.GetScan(0);
      double minR1 = sp.GetScan(1);
      double minR2 = sp.GetScan(2);
      double minR3 = sp.GetScan(3);
      double minR4 = sp.GetScan(4);
      double minR5 = sp.GetScan(5);
      double minR6 = sp.GetScan(6);
      double minR7 = sp.GetScan(7);
      double minL = lp.GetRange(327);

      std::cout << "Sonar(0): " << minR << std::endl
		<< "Sonar(1): " << minR1 << std::endl
		<< "Sonar(2): " << minR2 << std::endl
		<< "Sonar(3): " << minR3 << std::endl
		<< "Sonar(4): " << minR4 << std::endl
		<< "Sonar(5): " << minR5 << std::endl
		<< "Sonar(6): " << minR6 << std::endl
		<< "Sonar(7): " << minR7 << std::endl
                << "Laser(327): " << minL << std::endl
                << std::endl;
      /*
      if(minR <= 3.0){
	std::cout << "Uh-oh!\n";
	newspeed = 0.2;
	newturnrate = 0.375;
      }
      else if(minL <= 3.0){
	std::cout << "Uh-oh!\n";
	newspeed = 0.2;
	newturnrate = -0.375;
      }
      else{
	newspeed = 0.5;
	newturnrate = 0;
      }

      pp.SetSpeed(newspeed, newturnrate);
      //getchar();
      */

      /*
      uint count = lp.GetCount();
      for (uint j=0; j < count/2; ++j)  // find the minimum on the right side
      {
        if (minR > lp[j]){
	minR = lp[j];
	rloc=j;
	}
      }

      for (uint j = count/2; j < count; ++j) // find the minimum on the left side
      {
        if (minL > lp[j]){
	minL = lp[j];
	lloc=j;
	}
      }
      std::cout << "minR: " << minR
                << "minL: " << minL
                << std::endl;

      double l = (1e5*minR)/500-100;  // calc distance?
      double r = (1e5*minL)/500-100;

      if (l > 100)  // set a maximum
        l = 100;
      if (r > 100)
        r = 100;
 
      newspeed = (r+l)/1e3;  // determine the new speed

      newturnrate = (r-l);
      newturnrate = limit(newturnrate, -40.0, 40.0);
      newturnrate = dtor(newturnrate);
		
      if(lloc==count/2){
	newturnrate=1.5;
	newspeed=0;
	pp.SetSpeed(newspeed, newturnrate);
	std::cin >> temp;
      }
      */
    }
  }

  catch (PlayerCc::PlayerError e)
  {
    std::cerr << e << std::endl;
    return -1;
  }
}
